/* The register map is provided at
 * https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
 *
 * Mapping of the different gyro and accelero configurations:
 *
 * GYRO_CONFIG_[0,1,2,3] range = +- [250, 500,1000,2000] deg/s
 *                       sensi =    [131,65.5,32.8,16.4] bit/(deg/s)
 *
 * ACC_CONFIG_[0,1,2,3] range = +- [    2,   4,   8,  16] times the gravity (9.81 m/s^2)
 *                      sensi =    [16384,8192,4096,2048] bit/gravity
 */

#ifndef MPU6050_LIGHT_H
#define MPU6050_LIGHT_H

#include "Arduino.h"
#include "Wire.h"

#define CHIP_MPU6050 0x01
#define CHIP_LSM6DS3 0x02

#define LSM6DS3_ADDR 0x6A
#define LSM6DS3_WHO_AM_I 0x0F
#define LSM6DS3_WHO_AM_I_VALUE 0x6A
#define LSM6DS3_OUT_TEMP_REGISTER 0x20/*温度，2byte*/ 
#define LSM6DS3_OUT_G_REGISTER 0x22	 /*重力加速度 6byte*/ 
#define LSM6DS3_OUT_XL_REGISTER 0x28 /* 线加速度 6byte*/
#define LSM6DS3_OUT_STATUS_MASK_TEMP  (1<<2)
#define LSM6DS3_OUT_STATUS_MASK_G  (1<<1)
#define LSM6DS3_OUT_STATUS_MASK_XL (1<<0)
#define LSM6DS3_ACC_LSB_2G_PER       0.061

#define LSM6DS3_TEMP_LSB_PER_DEG 256.0
#define LSM6DS3_TEMP_OFFSET_DEG 25

#define MPU6050_ADDR 0x68
#define MPU6050_SMPLRT_DIV_REGISTER 0x19
#define MPU6050_CONFIG_REGISTER 0x1a
#define MPU6050_GYRO_CONFIG_REGISTER 0x1b
#define MPU6050_ACCEL_CONFIG_REGISTER 0x1c
#define MPU6050_PWR_MGMT_1_REGISTER 0x6b

#define MPU6050_GYRO_OUT_REGISTER 0x43
#define MPU6050_ACCEL_OUT_REGISTER 0x3B

#define RAD_2_DEG 57.29578 // [deg/rad]
#define CALIB_OFFSET_NB_MES 500
#define TEMP_LSB_2_DEGREE 340.0 // [bit/celsius]
#define TEMP_LSB_OFFSET 12412.0

#define DEFAULT_GYRO_COEFF 0.98

class MPU6050
{
public:
	// INIT and BASIC FUNCTIONS
	MPU6050(TwoWire &w);
	/**
	 * 重力加速度1 // range = +- 500 deg/s
	 * 加速度计0  // range = +- 2 g
	 */
	byte begin(int gyro_config_num = 1, int acc_config_num = 0);

	byte writeData(byte reg, byte data);
	byte readData(byte reg);

	void calcOffsets(bool is_calc_gyro = true, bool is_calc_acc = true);
	void calcGyroOffsets() { calcOffsets(true, false); }; // retro-compatibility with v1.0.0
	void calcAccOffsets() { calcOffsets(false, true); };  // retro-compatibility with v1.0.0

	void setAddress(uint8_t addr) { address = addr; };
	uint8_t getAddress() { return address; };

	// MPU CONFIG SETTER
	byte setGyroConfig(int config_num);
	byte setAccConfig(int config_num);

	void setGyroOffsets(float x, float y, float z);
	void setAccOffsets(float x, float y, float z);

	void setFilterGyroCoef(float gyro_coeff);
	void setFilterAccCoef(float acc_coeff);

	// MPU CONFIG GETTER
	float getGyroXoffset() { return gyroXoffset; };
	float getGyroYoffset() { return gyroYoffset; };
	float getGyroZoffset() { return gyroZoffset; };

	float getAccXoffset() { return accXoffset; };
	float getAccYoffset() { return accYoffset; };
	float getAccZoffset() { return accZoffset; };

	float getFilterGyroCoef() { return filterGyroCoef; };
	float getFilterAccCoef() { return 1.0 - filterGyroCoef; };

	// DATA GETTER
	float getTemp() { return temp; };

	float getAccX() { return accX; };
	float getAccY() { return accY; };
	float getAccZ() { return accZ; };

	float getGyroX() { return gyroX; };
	float getGyroY() { return gyroY; };
	float getGyroZ() { return gyroZ; };

	float getAccAngleX() { return angleAccX; };
	float getAccAngleY() { return angleAccY; };

	float getAngleX() { return angleX; };
	float getAngleY() { return angleY; };
	float getAngleZ() { return angleZ; };

	// INLOOP UPDATE
	void fetchData(); // user should better call 'update' that includes 'fetchData'
	void update();

	// UPSIDE DOWN MOUNTING
	bool upsideDownMounting = false;

private:
	TwoWire *wire;
	uint8_t address = MPU6050_ADDR; // 0x68 or 0x69
	float gyro_lsb_to_degsec, acc_lsb_to_g;
	float gyroXoffset, gyroYoffset, gyroZoffset;
	float accXoffset, accYoffset, accZoffset;
	float temp, accX, accY, accZ, gyroX, gyroY, gyroZ;
	float angleAccX, angleAccY;
	float angleX, angleY, angleZ;
	unsigned long preInterval;
	float filterGyroCoef; // complementary filter coefficient to balance gyro vs accelero data to get angle
};

#endif
